#include "Signal.h"

const double k_w = 3.3829e-12;
const double a_w = 0.6042;
const double E_w = 2.7e-3; // Unit = EV
const double k_c = 1.0274e-13;
const double a_c = 0.1669;
const double E_c = 1.9e-3; // Unit = EV
const double a_v = 15.55493;
const double k_v = 20.64504;
const double a_t = 0.02332;
const double k_t = 0.0261;
const double k_B = 8.617343e-5; // Boltzmann's constant, Unit = ev/K

// -----------------------------------------------------------
// C2 = C2(T,W)
class eq_C2:public MathFunction{
private:
  // parameters
  int LatticeSize;

  // Constant

public:
  eq_C2();
  virtual ~eq_C2();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(const double *para);
};

eq_C2::eq_C2():MathFunction(2,1){
  LatticeSize = 100;
}

eq_C2::~eq_C2(){}

double eq_C2::value(const double *x){
  double T = x[0];
  double W = x[1];
  return k_c*exp(k_B*T/E_c)*pow(W,-a_c)/(LatticeSize*LatticeSize);
}

void eq_C2::gradient(const double *x,double *grad){
  double T = x[0];
  double W = x[1];
  
  grad[0] = (k_B/E_c)*k_c*exp(k_B*T/E_c)*pow(W,-a_c)/(LatticeSize*LatticeSize);
  grad[1] = k_c*exp(k_B*T/E_c)*(-a_c)*pow(W,-a_c-1)/(LatticeSize*LatticeSize);
}

bool eq_C2::parameterize(const double *para){
  LatticeSize = int(para[0]);
  return true;
}

//eq_c2 c2;
// ------------------------------------------------------------
// sigma^2 = sigma^2(T,W)
class eq_sigma2:public MathFunction{
private:
  double LatticeSize;
public:
  eq_sigma2();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(const double *para);
};

eq_sigma2::eq_sigma2():MathFunction(2,1){
  LatticeSize = 100;
}

double eq_sigma2::value(const double *x){
  double T = x[0];
  double W = x[1];
  double index = (a_t+k_t*W)*T-a_v-k_v*W;
  return pi*pi/(LatticeSize*LatticeSize)*(1+exp(index));
  //  return (T0_W0+T0_W1*W)+(T1_W0+T1_W1*W)*T+(T2_W0+T2_W1*W)*T*T+(T3_W0+T3_W1*W)*T*T*T;
}

void eq_sigma2::gradient(const double *x,double *grad){
  double T = x[0];
  double W = x[1];
  
  grad[0]= pi*pi*exp((a_t+k_t*W)*T-a_v-k_v*W)*(a_t+k_t*W)/(LatticeSize*LatticeSize);
  grad[1]= pi*pi*exp((a_t+k_t*W)*T-a_v-k_v*W)*(k_t*T-k_v)/(LatticeSize*LatticeSize);
  // return T0_W1+T1_W1*T+T2_W1*T*T+T3_W1*T*T*T;
}

bool eq_sigma2::parameterize(const double *para){
  LatticeSize = int(para[0]);
  return true;
}

//eq_sigma2 sigma2;
// --------------------------------------------------------------
// cov_z_p = cov_z_p(T,W) = cov_z_p(C2(T,W),sigma2(T,W))
class eq_cov_z_p:public MathFunction{
private:
  // parameters
  int p;
  int m;
  int n;
  double dt;
  double cov_z_m;

  // auxillary variables
  int index;
  eq_C2 f_c2;
  eq_sigma2 f_sigma2;

  // Constant
  static const double XYmax = 8000;
public:
  eq_cov_z_p();
  ~eq_cov_z_p();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(int para_ID,double para_val);
};

eq_cov_z_p::eq_cov_z_p():MathFunction(2,5){
  p = 1;
  m = 5;
  n = 0;
  dt = 100;
  cov_z_m = 0;
}

eq_cov_z_p::~eq_cov_z_p(){
  
}

double eq_cov_z_p::value(const double *x){
  double T = x[0];
  double W = x[1];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);

  double lambda = -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
  double temp1 = exp(2*lambda*dt);
  double temp2 = sigma2*(temp1-1)/(2*lambda);
  double cov_z_p = cov_z_m*temp1+temp2;
  return cov_z_p;
}

void eq_cov_z_p::gradient(const double *x,double *grad){
  double T = x[0];
  double W = x[1];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);
  double y[2];    // y[0] = C2, y[1] = sigma2
  y[0] = C2;
  y[1] = sigma2;  

  double grad_C2[2];
  double grad_sigma2[2];
  f_c2.gradient(x,grad_C2);
  f_sigma2.gradient(x,grad_sigma2);

  double lambda = -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
  double temp1 = exp(2*lambda*dt);
  double dcov_z_dlambda =  2*temp1*cov_z_m*dt
	      +sigma2*(4*lambda*temp1*dt-2*(temp1-1))/(4*lambda*lambda);
  double dlambda_dC2 = 4*pi*pi*(m*m+n*n)/(XYmax*XYmax);
  double dcov_z_dC2 = dcov_z_dlambda*dlambda_dC2;
  double dcov_z_dsigma2 = (temp1-1)/(2*lambda);

  grad[0]= grad_C2[0]*dcov_z_dC2+grad_sigma2[0]*dcov_z_dsigma2;
  grad[1]= grad_C2[1]*dcov_z_dC2+grad_sigma2[1]*dcov_z_dsigma2;

  cout << "grad_C2[0]:" << grad_C2[0] << "\n";
  cout << "grad_C2[1]:" << grad_C2[1] << "\n";
  cout << "grad_sigma2[0]:" << grad_sigma2[0] << "\n";
  cout << "grad_sigma2[1]:" << grad_sigma2[1] << "\n";
  cout << "dcov_z_dC2:" << dcov_z_dC2 << "\n";
  cout << "dcov_z_dsigma2:" << dcov_z_dsigma2 << "\n";
}

bool eq_cov_z_p::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    p = int(para_val);break;
  case 1:
    m = int(para_val);break;
  case 2:
    n = int(para_val);break;
  case 3:
    dt = para_val;break;
  case 4:
    cov_z_m = para_val;break;
  }
  return true;
}

// -------------------------------------------------------------------
// z2_p = z2_p(T,W,A) = z2_p(C2(T,W),sigma2(T,W),W_pmn(A))
class eq_z2_p:public MathFunction{
private:
  // parameters
  int p;
  int m;
  int n;
  double dt;
  double cov_z_m;
  double mean_z_m;
  double Freq;

  // auxillary variables
  int index;
  eq_C2 f_c2;
  eq_sigma2 f_sigma2;

  // Constant
  static const double XYmax = 8000;
public:
  eq_z2_p();
  ~eq_z2_p();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(int para_ID,double para_val);
  virtual bool parameterize(const double* para);
  double eq_W_pmn(double W,double A);
  double eq_dW_pmn_dA();
};

eq_z2_p::eq_z2_p():MathFunction(3,7){
  p = 1;
  m = 5;
  n = 0;
  dt = 100;
  cov_z_m = 0;
  mean_z_m = 0;
  Freq = 5;
}

eq_z2_p::~eq_z2_p(){
  
}

double eq_z2_p::value(const double *x){
  double T = x[0];
  double W = x[1];
  double A = x[2];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);
  double w_pmn = eq_W_pmn(W,A);

  double lambda = -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
  double temp1 = exp(2*lambda*dt);
  double temp2 = sigma2*(temp1-1)/(2*lambda);
  double temp3 = exp(lambda*dt);
  double temp4 = (temp3-1)/lambda;

  double cov_z_p = cov_z_m*temp1+temp2;
  double mean_z_p = mean_z_m*temp3+w_pmn*(temp3-1)/lambda;
  return cov_z_p+mean_z_p*mean_z_p;
}

void eq_z2_p::gradient(const double *x,double *grad){
  double T = x[0];
  double W = x[1];
  double A = x[2];

  double C2 = f_c2.value(x);
  double sigma2 = f_sigma2.value(x);
  double w_pmn = eq_W_pmn(W,A);

  double lambda = -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
  double temp1 = exp(2*lambda*dt);
  double temp2 = sigma2*(temp1-1)/(2*lambda);
  double temp3 = exp(lambda*dt);
  double temp4 = (temp3-1)/lambda;
  double mean_z_p = mean_z_m*temp3+w_pmn*(temp3-1)/lambda;

  double grad_C2[2];
  double grad_sigma2[2];
  f_c2.gradient(x,grad_C2);
  f_sigma2.gradient(x,grad_sigma2);
  double dw_pmn_dW;
  if(p==1&&m==0&&n==0){
    dw_pmn_dW = XYmax;
  }
  else{
    dw_pmn_dW = 0.0;
  }
  double dw_pmn_dA = eq_dW_pmn_dA();

  double dlambda_dT = -4*pi*pi*(m*m+n*n)/(XYmax*XYmax)*grad_C2[0];
  double dlambda_dW = -4*pi*pi*(m*m+n*n)/(XYmax*XYmax)*grad_C2[1];

  double dcov_z_dlambda =  2*temp1*cov_z_m*dt
    +sigma2*(4*lambda*temp1*dt-2*(temp1-1))/(4*lambda*lambda);
  double dlambda_dC2 = -4*pi*pi*(m*m+n*n)/(XYmax*XYmax);
  double dcov_z_dC2 = dcov_z_dlambda*dlambda_dC2;
  double dcov_z_dsigma2 = (temp1-1)/(2*lambda);

  double dcov_z_dT = dcov_z_dC2*grad_C2[0]+dcov_z_dsigma2*grad_sigma2[0];
  double dcov_z_dW = dcov_z_dC2*grad_C2[1]+dcov_z_dsigma2*grad_sigma2[1];
  double dcov_z_dA = 0.0;  

  double dmean_z_dlambda = temp3*mean_z_m*dt*dlambda_dT
    +w_pmn*(lambda*temp3*dt-(temp3-1))/(lambda*lambda);
  double dmean_z_dw_pmn  = (temp3-1)/lambda;
  double dmean_z_dT = dmean_z_dlambda*dlambda_dT;
  double dmean_z_dW = dmean_z_dlambda*dlambda_dW+dmean_z_dw_pmn*dw_pmn_dW;
  double dmean_z_dA = dmean_z_dw_pmn*dw_pmn_dA;

  grad[0]= dcov_z_dT+2*mean_z_p*dmean_z_dT;
  grad[1]= dcov_z_dW+2*mean_z_p*dmean_z_dW;
  grad[2]= dcov_z_dA+2*mean_z_p*dmean_z_dA;
  /*
  cout << "********************************************\n";
  cout << "[p,m,n] = [" << p << ", "  << m << ", " << n << "]\n";
  cout << "grad_C2[0]    :" << grad_C2[0] << "\n";
  cout << "grad_C2[1]    :" << grad_C2[1] << "\n";
  cout << "grad_sigma2[0]:" << grad_sigma2[0] << "\n";
  cout << "grad_sigma2[1]:" << grad_sigma2[1] << "\n";
  cout << "dcov_z_dC2    :" << dcov_z_dC2 << "\n";
  cout << "dcov_z_dsigma2:" << dcov_z_dsigma2 << "\n";
  */
}

bool eq_z2_p::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    p = int(para_val);break;
  case 1:
    m = int(para_val);break;
  case 2:
    n = int(para_val);break;
  case 3:
    dt = para_val;break;
  case 4:
    cov_z_m = para_val;break;
  case 5:
    mean_z_m = para_val;break;
  case 6:
    Freq = para_val;break;
  }
  return true;
}

bool eq_z2_p::parameterize(const double *para){
  p = int(para[0]);
  m = int(para[1]);
  n = int(para[2]);
  dt = para[3];
  cov_z_m = para[4];
  mean_z_m = para[5];
  Freq = para[6];
  return true;
}

double eq_z2_p::eq_W_pmn(double W,double A){
  double w_pmn;
  switch(p){
  case 0:
    w_pmn = 0;
    break;
  case 1:
    if(n!=0){
      w_pmn = 0;
    }
    else{
      if(m==0){
	w_pmn = W*XYmax+A*XYmax/(2*Freq*pi)*(1-cos(2*Freq*pi));
      }
      else{
	if(m==Freq){
	  w_pmn = 0;
	}
	else{
	  w_pmn = SQRT_TWO*A*XYmax*Freq*(cos(2*Freq*pi)-1)/(2*pi*(m*m-Freq*Freq));
	}
      }
    }
    break;
  case 2:
    if(m!=0&&n==0){
      if(m==Freq){
	w_pmn = SQRT_TWO*A*XYmax/2;
      }
      else{
	w_pmn = SQRT_TWO*XYmax*A*m*sin(2*Freq*pi)/(2*pi*(Freq*Freq-m*m));
      }
    }
    else{
      w_pmn = 0;
    }
    break;
  case 3:
    w_pmn= 0;
    break;
  }
  return w_pmn;
}

double eq_z2_p::eq_dW_pmn_dA(){
  double dW_dA;
  switch(p){
  case 0:
    dW_dA = 0; 
    break;
  case 1:
    if(m==0&&n==0){
      dW_dA = XYmax*(1-cos(2*Freq*pi))/(2*Freq*pi);
    }
    else if(m!=Freq && n==0){
      dW_dA = SQRT_TWO*XYmax*Freq*(cos(2*Freq*pi)-1)/(2*pi*(m*m-Freq*Freq));
    }
    else{
      dW_dA = 0;
    }
    break;
  case 2:
    if(m!=0&&n==0){
      if(m==Freq){
	dW_dA = SQRT_TWO*XYmax/2;
      }
      else{
	dW_dA = SQRT_TWO*XYmax*m*sin(2*Freq*pi)/(2*pi*(Freq*Freq-m*m));
      }
    }
    else{
      dW_dA = 0;
    }
    break;
  case 3:
    dW_dA = 0; 
    break;
  }
  return dW_dA;
}  
// --------------------------------------------------------------
// cov_z_p = cov_z_p2(C2,sigma2)
class eq_cov_z_p2:public MathFunction{
private:
  // parameters
  int p;
  int m;
  int n;
  double dt;
  double cov_z_m;
  // Constant
  static const double XYmax = 8000;
public:
  eq_cov_z_p2();
  ~eq_cov_z_p2();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(int para_ID,double para_val);
};

eq_cov_z_p2::eq_cov_z_p2():MathFunction(2,5){
  p = 1;
  m = 5;
  n = 0;
  dt = 100;
  cov_z_m = 0;
}

eq_cov_z_p2::~eq_cov_z_p2(){
  
}

double eq_cov_z_p2::value(const double *x){
  double C2 = x[0];
  double sigma2 = x[1];
  double lambda = -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
  double temp1 = exp(2*lambda*dt);
  double temp2 = sigma2*(temp1-1)/(2*lambda);
  double cov_z_p = cov_z_m*temp1+temp2;
  return cov_z_p;
}

void eq_cov_z_p2::gradient(const double *x,double *grad){
  double C2 = x[0];
  double sigma2 = x[1];

  double lambda = -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
  double temp1 = exp(2*lambda*dt);
  double dcov_z_dlambda =  2*temp1*cov_z_m*dt
	      +sigma2*(4*lambda*temp1*dt-2*(temp1-1))/(4*lambda*lambda);
  double dlambda_dC2 = 4*pi*pi*(m*m+n*n)/(XYmax*XYmax);
  double dcov_z_dC2 = dcov_z_dlambda*dlambda_dC2;
  double dcov_z_dsigma2 = (temp1-1)/(2*lambda);

  grad[0]= dcov_z_dC2;
  grad[1]= dcov_z_dsigma2;
}

bool eq_cov_z_p2::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    p = int(para_val);break;
  case 1:
    m = int(para_val);break;
  case 2:
    n = int(para_val);break;
  case 3:
    dt = para_val;break;
  case 4:
    cov_z_m = para_val;break;
  }
  return true;
}  
// ----------------------------------
// f1(z) = exp(z)
class eq_fun1:public MathFunction{
private:
public:
  eq_fun1();
  virtual ~eq_fun1();
  virtual double value(const double* x);
  virtual void gradient(const double *x, double *grad);
};

eq_fun1::eq_fun1():MathFunction(1,0){
}

eq_fun1::~eq_fun1(){}

double eq_fun1::value(const double *x){
  return exp(x[0]);
}

void eq_fun1::gradient(const double *x, double *grad){
  grad[0] = exp(x[0]);
}

// ----------------------------------
// f2(x1,x2) = x1^2+x2^2
class eq_fun2:public MathFunction{
private:
public:
  eq_fun2();
  virtual ~eq_fun2();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
};

eq_fun2::eq_fun2():MathFunction(2,0){
}

eq_fun2::~eq_fun2(){}

double eq_fun2::value(const double *x){
  return x[0]*x[0]+x[1]*x[1];
}

void eq_fun2::gradient(const double *x,double *grad){
  grad[0] = 2*x[0];
  grad[1] = 2*x[1];
}
// ---------------------------------------------------------------
// f3(x1,x2) = exp(x1^2+x2^2) = f1(f2(x1,x2))
// \frac{\partial f3}{\partial x1} = exp(x1^2+x2^2)*2*x1 = (\frac{\partial f1}{\partial z}|z = x1^2+x2^2)(\frac{\partial f2}{\partial x1})
// \frac{\partial f3}{\partial x2} = exp(x1^2+x2^2)*2*x2 = (\frac{\partial f1}{\partial z}|z = x1^2+x2^2)(\frac{\partial f2}{\partial x2})
class eq_fun3:public MathFunction{
private:
  eq_fun1 fun1;
  eq_fun2 fun2;
public:
  eq_fun3();
  virtual ~eq_fun3();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
};

eq_fun3::eq_fun3():MathFunction(2,0),fun1(),fun2(){
}

eq_fun3::~eq_fun3(){}

double eq_fun3::value(const double *x){
  double z[1];
  z[0] = fun2.value(x);

  return fun1.value(z);
}

void eq_fun3::gradient(const double *x,double *grad){
  double z[1];
  z[0] = fun2.value(x);

  double grad_fun1[1];
  fun1.gradient(z,grad_fun1);
  double grad_fun2[2];
  fun2.gradient(x,grad_fun2);

  grad[0] = grad_fun1[0]*grad_fun2[0];
  grad[1] = grad_fun1[0]*grad_fun2[1];
}
// ---------------------------------------------------------------
// r2_f = r2_f(T,W,A)
class eq_r2_f:public MathFunction{
private:
  // Parameters
  int mode;
  double XYmax;
  double Freq;
  int LatticeSize;
  double dt;
  double *cov_z_m[4];
  double *mean_z_m[4];
  
  // Auxillary variables
  eq_z2_p z2_p;
public:
  eq_r2_f(double i_dt = 100,int i_mode = 50,double i_XYmax = 8000,
	  int i_LatticeSize = 100,double i_Freq=5);
  virtual ~eq_r2_f();
  virtual double value(const double *x);
  virtual void gradient(const double *x, double *grad);
  virtual bool parameterize(int para_ID, double para_val);
};

eq_r2_f::eq_r2_f(double i_dt,int i_mode,double i_XYmax, 
		 int i_LatticeSize,double i_Freq):
  MathFunction(3,5),dt(i_dt),mode(i_mode),XYmax(i_XYmax),
  LatticeSize(i_LatticeSize),Freq(i_Freq){
  int i,j;
  int dim = (mode+1)*(mode+1);
  for(i=0;i<4;i++){
    cov_z_m[i] = new double[dim];
    mean_z_m[i] = new double[dim];
    for(j=0;j<dim;j++){
      cov_z_m[i][j] = 0.0;
      mean_z_m[i][j] = 0.0;
    }
  }
}

eq_r2_f::~eq_r2_f(){
  int i;
  for(i=0;i<4;i++){
    delete []cov_z_m[i];
    delete []mean_z_m[i];
  }
}

double eq_r2_f::value(const double *x){
  int    p,m,n;
  double r2_f = 0.0;
  int    index;
  double para_z2_p[7];

  para_z2_p[3] = dt;
  para_z2_p[6] = Freq;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = m*(mode+1)+n;
	for(p=0;p<4;p++){
	  para_z2_p[0] = p;
	  para_z2_p[1] = m;
	  para_z2_p[2] = n;
	  para_z2_p[4] = cov_z_m[p][index];
	  para_z2_p[5] = mean_z_m[p][index];
	  z2_p.parameterize(para_z2_p);
	  r2_f += z2_p.value(x);
	}
      }
    }
  }
  return r2_f/=(XYmax*XYmax);
}

void eq_r2_f::gradient(const double *x,double *grad){
  int    p,m,n;
  int    index;
  double para_z2_p[7];
  double grad_z2_p[3];

  para_z2_p[3] = dt;
  para_z2_p[6] = Freq;
  grad[0] = 0.0;
  grad[1] = 0.0;
  grad[2] = 0.0;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = m*(mode+1)+n;
	for(p=0;p<4;p++){
	  para_z2_p[0] = p;
	  para_z2_p[1] = m;
	  para_z2_p[2] = n;
	  para_z2_p[4] = cov_z_m[p][index];
	  para_z2_p[5] = mean_z_m[p][index];
	  z2_p.parameterize(para_z2_p);
	  z2_p.gradient(x,grad_z2_p);
	  //	  cout << "[p,m,n]=" << p << ", " << m << ", " << n << "\n";
	  //	  cout << "grad = [" << grad_z2_p[0] << ", " << grad_z2_p[1] << ", " << grad_z2_p[2] << "]\n";
	  grad[0] += grad_z2_p[0];
	  grad[1] += grad_z2_p[1];
	  grad[2] += grad_z2_p[2];
	}
      }
    }
  }
  grad[0] /= (XYmax*XYmax);
  grad[1] /= (XYmax*XYmax);
  grad[2] /= (XYmax*XYmax);
}  

bool eq_r2_f::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    mode = int(para_val);break;
  case 1:
    XYmax = para_val;break;
  case 2:
    Freq = para_val;break;
  case 3:
    LatticeSize = int(para_val);break;
  case 4:
    dt = para_val;break;
  default:
    cerr << "er_r2_f::parameterize(): invalid PARA_ID\n";
  }
  return true;
}
// ---------------------------------------------------------------
class eq_m2_f:public MathFunction{
private:
  // Parameters
  int mode;
  double XYmax;
  double Freq;
  int LatticeSize;
  double dt;
  double *cov_z_m[4];
  double *mean_z_m[4];
  double *K[4];
  
  // Auxillary variables
  eq_z2_p z2_p;
public:
  eq_m2_f(double i_dt = 100,int i_mode = 50,double i_XYmax = 8000,
	  int i_LatticeSize = 100, double i_Freq =5);
  virtual ~eq_m2_f();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  //  virtual bool parameterize(const double *para);
  virtual bool parameterize(int para_ID,double para_val);
};

eq_m2_f::eq_m2_f(double i_dt,int i_mode,double i_XYmax,
		 int i_LatticeSize, double i_Freq):
  MathFunction(3,5),dt(i_dt),mode(i_mode),XYmax(i_XYmax),
  LatticeSize(i_LatticeSize),Freq(i_Freq){
  int i,m,n,index;
  int dim = (mode+1)*(mode+1);
  double temp;
  for(i=0;i<4;i++){
    cov_z_m[i] = new double[dim];
    mean_z_m[i] = new double[dim];
    K[i] = new double[dim];
    for(m=0;m<=mode;m++){
      for(n=0;n<=mode;n++){
	index = m*(mode+1)+n;
	cov_z_m[i][index] = 0.0;
	mean_z_m[i][index] = 0.0;
	temp = sin(pi*m/LatticeSize);
	K[i][index] = 4*temp*temp/(XYmax*XYmax);
      }
    }
  }
}

eq_m2_f::~eq_m2_f(){
  int i;
  for(i=0;i<4;i++){
    delete []cov_z_m[i];
    delete []mean_z_m[i];
    delete []K[i];
  }
}


double eq_m2_f::value(const double *x){
  int    p,m,n;
  double m2_f = 0.0;
  int    index;
  double para_z2_p[7];

  para_z2_p[3] = dt;
  para_z2_p[6] = Freq;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = m*(mode+1)+n;
	for(p=0;p<4;p++){
	  para_z2_p[0] = p;
	  para_z2_p[1] = m;
	  para_z2_p[2] = n;
	  para_z2_p[4] = cov_z_m[p][index];
	  para_z2_p[5] = mean_z_m[p][index];
	  z2_p.parameterize(para_z2_p);
	  m2_f += K[p][index]*z2_p.value(x);
	}
      }
    }
  }
  return m2_f;
}

void eq_m2_f::gradient(const double *x,double *grad){
  int    p,m,n;
  int    index;
  double para_z2_p[7];
  double grad_z2_p[3];

  para_z2_p[3] = dt;
  para_z2_p[6] = Freq;
  grad[0] = 0.0;
  grad[1] = 0.0;
  grad[2] = 0.0;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = m*(mode+1)+n;
	for(p=0;p<4;p++){
	  para_z2_p[0] = p;
	  para_z2_p[1] = m;
	  para_z2_p[2] = n;
	  para_z2_p[4] = cov_z_m[p][index];
	  para_z2_p[5] = mean_z_m[p][index];
	  z2_p.parameterize(para_z2_p);
	  z2_p.gradient(x,grad_z2_p);
	  //	  cout << "[p,m,n]=" << p << ", " << m << ", " << n << "\n";
	  //	  cout << "grad = [" << grad_z2_p[0] << ", " << grad_z2_p[1] << ", " << grad_z2_p[2] << "]\n";
	  grad[0] += K[p][index]*grad_z2_p[0];
	  grad[1] += K[p][index]*grad_z2_p[1];
	  grad[2] += K[p][index]*grad_z2_p[2];
	}
      }
    }
  }
}  

bool eq_m2_f::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    mode = int(para_val);break;
  case 1:
    XYmax = para_val;break;
  case 2:
    Freq = para_val;break;
  case 3:
    LatticeSize = int(para_val);break;
  case 4:
    dt = para_val;break;
  default:
    cerr << "er_r2_f::parameterize(): invalid PARA_ID\n";
  }
  return true;
}

// ---------------------------------------------------------------
// f = f(T,W,A) = q_r*(r2_set-r2_f)^2
class eq_f:public MathFunction{
private:
  double r2_set;
  double q_r;
  double m2_set;
  double q_m;
  eq_r2_f f_r2_f;
  eq_m2_f f_m2_f;
public:
  eq_f(double i_r2_set = 900,double i_q_r=1.0,double i_m2_set = 36,double i_q_m=1.0);
  virtual ~eq_f();
  virtual double value(const double *x);
  virtual void gradient(const double *x,double *grad);
  virtual bool parameterize(int para_ID,double para_val);
};

eq_f::eq_f(double i_r2_set,double i_q_r,double i_m2_set,double i_q_m):
  MathFunction(3,4),r2_set(i_r2_set),q_r(i_q_r),m2_set(i_m2_set),q_m(i_q_m)
{
  
}

eq_f::~eq_f(){
  
}

double eq_f::value(const double *x){
  double r2_f = f_r2_f.value(x);
  double m2_f = f_m2_f.value(x);
  return q_r*(r2_set-r2_f)*(r2_set-r2_f)+q_m*(m2_set-m2_f)*(m2_set-m2_f);
}

void eq_f::gradient(const double *x,double *grad){
  double grad_r2_f[3];
  f_r2_f.gradient(x,grad_r2_f);
  double r2_f = f_r2_f.value(x);

  double grad_m2_f[3];
  f_m2_f.gradient(x,grad_m2_f);
  double m2_f = f_m2_f.value(x);
  grad[0] = -2*q_r*(r2_set-r2_f)*grad_r2_f[0]-2*q_m*(m2_set-m2_f)*grad_m2_f[0];
  grad[1] = -2*q_r*(r2_set-r2_f)*grad_r2_f[1]-2*q_m*(m2_set-m2_f)*grad_m2_f[1];
  grad[2] = -2*q_r*(r2_set-r2_f)*grad_r2_f[2]-2*q_m*(m2_set-m2_f)*grad_m2_f[2];
}

bool eq_f::parameterize(int para_ID,double para_val){
  switch(para_ID){
  case 0:
    r2_set = para_val;break;
  case 1:
    q_r = para_val;break;
  case 2:
    m2_set = para_val;break;
  case 3:
    q_m = para_val;break;
  default:
    cerr << "eq_f::parameterize(): invalid PARA_ID\n";
  }
  return true;
}
// ---------------------------------------------------------------
int main(int argc, char** argv){
  double x[3];
  double epsilon[3];
  double value;
  double grad[2];
  double LatticeSize = 100;

  x[0] = atof(argv[1]);
  x[1] = atof(argv[2]);
  x[2] = atof(argv[3]);
  epsilon[0]= 1e-15;
  epsilon[1]= 1e-15;
  epsilon[2]= 1e-15;

  cout << "c2(T,W)\n";
  MathFunction* c2 = new eq_C2();
  c2->parameterize(&LatticeSize);
  value = c2->value(x);
  c2->gradient(x,grad);
  //  cout << "x = [" << x[0] << ", " << x[1] << "]\n";
  //cout << "c2 = " << value << "\n";
  //cout << "grad c2 = [" << grad[0] << ", " << grad[1] << "]\n";
  perturb_MathFunction(c2,x,epsilon);
  
  cout << "\nsigma2(T,W)\n";
  MathFunction* sigma2 = new eq_sigma2();
  perturb_MathFunction(sigma2,x,epsilon);

  /*MathFunction* f3 = new eq_fun3();
  x[0] = 1;
  x[1] = 2;
  epsilon[0]= 0.001*x[0];
  epsilon[1]= 0.001*x[1];  
  perturb_MathFunction(f3,x,epsilon);
  delete f3;*/

  cout << "\ncov_z_p(T,W)\n";
  MathFunction* cov_z_p = new eq_cov_z_p();
  cov_z_p->parameterize(0,2);
  cov_z_p->parameterize(1,5);
  cov_z_p->parameterize(2,0);
  perturb_MathFunction(cov_z_p,x,epsilon);

  cout << "\ncov_z_p2(T,W)\n";
  double y[2];
  double epsilon_y[2];
  MathFunction* cov_z_p2 = new eq_cov_z_p2();
  y[0] = c2->value(x);
  y[1] = sigma2->value(x);
  epsilon_y[0]= 1e-15;
  epsilon_y[1]= 1e-15;
  perturb_MathFunction(cov_z_p2,y,epsilon_y);

  cout << "z2_p(T,W,A)\n";
  MathFunction* z2_p = new eq_z2_p();
  int p,m,n;
  /*
  for(m=0;m<=50;m++){
    for(n=0;n<=50;n++){
      if(m!=0 || n!=0){
	for(p=0;p<4;p++){
	  z2_p->parameterize(0,p);
	  z2_p->parameterize(1,m);
	  z2_p->parameterize(2,n);
	  cout << "p=" << p << ", m=" << m << ", n=" << n << "\n";
	  if(perturb_MathFunction(z2_p,x,epsilon)!=0){
	    cout << "ERROR @ p=" << p << ", m=" << m << ", n=" << n << "\n";
	  }
	}
      }
    }
  }
  */
  z2_p->parameterize(0,2);
  z2_p->parameterize(1,5);
  z2_p->parameterize(2,0);
  double x1[3];
  double epsilon1[3];
  x1[0] = x[0];
  x1[1] = x[1];
  x1[2] = x[2];
  //  epsilon1[0] = 0.00001*x1[0];
  epsilon1[0] = 1e-13;
  epsilon1[1] = 1e-13;
  epsilon1[2] = 1e-13;
  perturb_MathFunction(z2_p,x1,epsilon1);
  // 12.2363, -45.0286, 0

  
  cout << "\n r2_f(T,W,A)\n";
  MathFunction* r2_f = new eq_r2_f();
  perturb_MathFunction(r2_f,x,epsilon1);
  double grad_r2_f[3];
  r2_f->gradient(x,grad_r2_f);

  cout << "\n m2_f(T,W,A)\n";
  MathFunction* m2_f = new eq_m2_f();
  perturb_MathFunction(m2_f,x,epsilon1);
  double grad_m2_f[3];
  m2_f->gradient(x,grad_m2_f);

  cout << "\n f(T,W,A)\n";
  MathFunction* cost_fun = new eq_f();
  perturb_MathFunction(cost_fun,x,epsilon1);

  double T;
  double W = 1;
  double A = 0.5;

  ofstream f_log("test_MathFunction_function.log");
  for(T = 580;T<=620;T=T+1.0){
    x[0] = T;
    x[1] = W;
    x[2] = A;
    f_log << setw(15) << T 
	  << setw(15) << W 
	  << setw(15) << A 
      //	  << setw(15) << cost_fun->value(x) 
	  << setw(15) << r2_f->value(x)
	  << setw(15) << z2_p->value(x)
	  << "\n";
  }
  
  delete c2;
  delete sigma2;
  delete cov_z_p;
  delete cov_z_p2;
  delete z2_p;
  delete r2_f;
  delete m2_f;
  delete cost_fun;
}
	      
